




#include<math.h>
#include<stdlib.h>



#include"sl_complex.h"



// 这里实现复数的运算，FFT，解一元n次方程（可以求解复数根）



/// <summary>
/// 打印复数
/// </summary>
/// <param name="c"></param>
void complex_print(complex c)
{
	printfs(" [");
	printfs(" %9f ,", c.real);
	printfs(" %9f ", c.imag);
	printfs(" ] ");
}


/// <summary>
/// 判断复数是否相等。复数没法直接比大小。
/// </summary>
/// <param name="A">复数</param>
/// <param name="B">复数</param>
/// <param name="precision">精度</param>
/// <returns>A与B的差在精度范围内，返回true，否则返回false</returns>
bool complex_isEqual(complex A, complex B, real_t precision)
{
	if (ABS(B.real - A.real) > precision) return false;
	else if (ABS(B.imag - A.imag) > precision) return false;
	else return true;
}

/// <summary>
/// 判断是否是零。 
/// </summary>
/// <param name="A">复数</param>
/// <param name="precision">精度</param>
/// <returns>A与0的差在精度范围内，返回true，否则返回false</returns>
bool complex_isZero(complex A, real_t precision)
{
	if (ABS(A.real) > precision) return false;
	else if (ABS(A.imag) > precision) return false;
	else return true;
}

/// <summary>
/// 计算复数模长
/// </summary>
/// <param name="A">复数</param>
/// <returns>复数的模长</returns>
real_t complex_mod(complex A) //返回复数的模.  complex modulus[数] 复数模量;
{
	return  sqrtf(A.real * A.real + A.imag * A.imag);
}
/// <summary>
/// 
/// </summary>
/// <param name="A">复数</param>
/// <returns>返回复数的模的平方</returns>
real_t complex_modSquare(complex A) //返回复数的模的平方
{
	return  A.real * A.real + A.imag * A.imag;
}
/// <summary>
/// 
/// </summary>
/// <param name="A">复数</param>
/// <returns>共轭复数</returns>
complex complex_conj(complex A)  // conjugate 共轭。得到共轭复数
{
	complex c;
	c.real = A.real;
	c.imag = -A.imag;
	return c;
}
/// <summary>
/// 复数加法
/// </summary>
/// <param name="a">复数</param>
/// <param name="b">复数</param>
/// <returns>两个复数的和 a+b</returns>
complex complex_add(complex a, complex b)
{
	complex c;
	c.real = a.real + b.real;
	c.imag = a.imag + b.imag;
	return c;
}
/// <summary>
/// 复数减法
/// </summary>
/// <param name="a">复数</param>
/// <param name="b">复数</param>
/// <returns>两个复数的差 a-b</returns>
complex complex_sub(complex a, complex b)
{
	complex c;
	c.real = a.real - b.real;
	c.imag = a.imag - b.imag;
	return c;
}
/// <summary>
/// 复数乘法
/// </summary>
/// <param name="a">复数</param>
/// <param name="b">复数</param>
/// <returns>两个复数的积 a*b</returns>
complex complex_mul(complex a, complex b)
{
	complex c;
	c.real = a.real * b.real - a.imag * b.imag;
	c.imag = a.real * b.imag + a.imag * b.real;
	return c;
}

/// <summary>
/// 复数乘实数
/// </summary>
/// <param name="a">复数</param>
/// <param name="b">实数</param>
/// <returns> a * b </returns>
complex complex_mul_real(complex a, real_t b)
{
	complex c;
	c.real = a.real * b;
	c.imag = a.imag * b;
	return c;
}


/// <summary>
/// 复数除法
/// </summary>
/// <param name="a">复数</param>
/// <param name="b">复数</param>
/// <returns>两个复数的商 a/b</returns>
complex complex_div(complex a, complex b)
{
	complex c;
	real_t d = complex_modSquare(b);
	c.real = (a.real * b.real + a.imag * b.imag) / d;
	c.imag = (-a.real * b.imag + a.imag * b.real) / d;
	return c;
}

/// <summary>
/// 复数除实数
/// </summary>
/// <param name="a">复数</param>
/// <param name="b">实数</param>
/// <returns>复数的商 a/b</returns>
complex complex_div_real(complex a, real_t b)
{
	complex c;
	c.real = a.real / b;
	c.imag = a.imag / b;
	return c;
}

//根据time_domain计算frequency_domain,w 是旋转因子缓存，长度是length的一半。

/// <summary>
/// 根据time_domain计算frequency_domain,w 是旋转因子缓存，长度是length的一半。
/// </summary>
/// <param name="time_domain">时域</param>
/// <param name="frequency_domain">频域</param>
/// <param name="w">旋转因子，中间计算量的缓存，长度是length的一半</param>
/// <param name="log2_length">长度的2为底的对数。</param>
void fft(complex* time_domain, complex* frequency_domain, complex* w, int log2_length)
{
	//complex* time_domain;  //时域。domain领域 。
	//complex* frequency_domain;  //频域
	//int log2_length;  //2的指数。  2 ^ log2_length = length

	int length = 1;  //数组长度，时域与频域的长度。	
	int i, j, k;  //循环用参数
	for (i = 0; i < log2_length; i++) {   //计算数组长度。
		length = length * 2;
	}

	int  r, Pre, Next;  //pre 和next 分别表示蝴蝶操作的上下两个点的序号。
	real_t  theta;
	//////////////////////////////倒位序
	for (j = 0; j < length; j++)
	{
		r = 0;
		for (i = 0; i < log2_length; i++)
		{
			if (j & (1 << i))
			{
				r += (1 << (log2_length - i - 1));
			}
		}
		frequency_domain[j].real = time_domain[r].real;   //r是j的倒位序
		frequency_domain[j].imag = time_domain[r].imag;
	}
	complex temp[2]; //蝴蝶操作中间变量
	//	complex* w = new complex[length / 2]; // 分配旋转因子存储空间
	for (i = 0; i < length / 2; i++)
	{
		theta = (-i) * 2 * PI / length;
		w[i].real = cosf(theta);
		w[i].imag = sinf(theta);
	}
	for (i = 0; i < log2_length; i++)   // 共 n 级蝴蝶操作
	{
		for (j = 0; j < (length >> (i + 1)); j++) // 0级有N/2份蝴蝶操作，1级有N/4份，2级N/8份，类推
		{
			for (k = 0; k < (1 << i); k++)// 蝴蝶操作，0级每份1次，1级每份2次，2级每份4次，类推
			{
				Pre = k + j * (1 << (i + 1));
				Next = k + j * (1 << (i + 1)) + (1 << i);
				temp[0] = frequency_domain[Pre];

				temp[1] = complex_mul(frequency_domain[Next], w[k * (length >> (i + 1))]);
				frequency_domain[Pre] = complex_add(temp[0], temp[1]);
				frequency_domain[Next] = complex_sub(temp[0], temp[1]);
			}
		}
	}
	// delete[] w;
}

/// <summary>
/// 根据frequency_domain计算time_domain
/// </summary>
/// <param name="time_domain">时域</param>
/// <param name="frequency_domain">频域</param>
/// <param name="w">旋转因子，中间计算量的缓存，长度是length的一半</param>
/// <param name="log2_length">长度的2为底的对数。</param>
void ifft(complex* time_domain, complex* frequency_domain, complex* w, int log2_length)
{
	//complex* time_domain;  //时域。domain领域 。
	//complex* frequency_domain;  //频域
	//int log2_length;  //2的指数。  2 ^ log2_length = length

	int length = 1;  //数组长度，时域与频域的长度。	
	int i, j, k;  //循环用参数
	for (i = 0; i < log2_length; i++) {   //计算数组长度。
		length = length * 2;
	}

	int  r, Pre, Next;  //pre 和next 分别表示蝴蝶操作的上下两个点的序号。
	real_t  theta;
	//////////////////////////////倒位序
	for (j = 0; j < length; j++)
	{
		r = 0;
		for (i = 0; i < log2_length; i++)
		{
			if (j & (1 << i))
			{
				r += (1 << (log2_length - i - 1));
			}
		}
		time_domain[j].real = frequency_domain[r].real;   //r是j的倒位序
		time_domain[j].imag = frequency_domain[r].imag;
	}
	complex temp[2]; //蝴蝶操作中间变量
	//complex* w = new complex[length / 2]; // 分配旋转因子存储空间
	for (i = 0; i < length / 2; i++)
	{
		theta = (i) * 2 * PI / length;  //这里i>0,即theta>0，是逆傅里叶变换。
		w[i].real = cosf(theta);
		w[i].imag = sinf(theta);
	}
	for (i = 0; i < log2_length; i++)   // 共 n 级蝴蝶操作
	{
		for (j = 0; j < (length >> (i + 1)); j++) // 0级有N/2份蝴蝶操作，1级有N/4份，2级N/8份，类推
		{
			for (k = 0; k < (1 << i); k++)// 蝴蝶操作，0级每份1次，1级每份2次，2级每份4次，类推
			{
				Pre = k + j * (1 << (i + 1));
				Next = k + j * (1 << (i + 1)) + (1 << i);
				temp[0] = time_domain[Pre];

				temp[1] = complex_mul(time_domain[Next], w[k * (length >> (i + 1))]);
				time_domain[Pre] = complex_add(temp[0], temp[1]);
				time_domain[Next] = complex_sub(temp[0], temp[1]);
			}
		}
	}
	for (i = 0; i < length; i++)
	{
		time_domain[i].real = time_domain[i].real / length;
		time_domain[i].imag = time_domain[i].imag / length;
	}
	//delete[] w;
}


/// <summary>
/// 测试傅里叶变换
/// </summary>
void test_FT()
{
	complex td[32];
	complex fd[32];
	complex cfd[32];
	complex w[32];
//	int ln2len = 5;
	printfs(" \r\n test fft ");
	for (int i = 0; i < 32; i++) {
		td[i].real = i;
		td[i].imag = 0;
	}

	for (int i = 0; i < 32; i++)
	{
		printfs("  \r\n ");
		complex_print(td[i]);
	}


	fft(td, fd, w, 5);
	for (int i = 0; i < 32; i++)
	{
		printfs("  \r\n ");
		complex_print(td[i]);
		printfs("   ");
		complex_print(fd[i]);
	}


	ifft(cfd, fd, w, 5);
	for (int i = 0; i < 32; i++)
	{
		printfs("  \r\n ");
		complex_print(fd[i]);
		printfs("   ");
		complex_print(cfd[i]);
	}
}



/// <summary>
/// 分配维纳滤波器内存
/// </summary>
/// <param name="w">维纳滤波器</param>
/// <param name="ln2len">滤波器处理的信号长度</param>
void wiener_allocate(wiener* w, int ln2len)
{
	w->log2_length = ln2len;
	w->length = 1;
	int i;  //循环用参数
	for (i = 0; i < w->log2_length; i++) {   //计算数组长度。
		w->length = w->length * 2;
	}

	w->signal = (complex*)malloc(w->length * sizeof(complex));
	w->noise = (complex*)malloc(w->length * sizeof(complex));
	w->observe = (complex*)malloc(w->length * sizeof(complex));
	w->signal_frequency = (complex*)malloc(w->length * sizeof(complex));
	w->noise_frequency = (complex*)malloc(w->length * sizeof(complex));
	w->observe_frequency = (complex*)malloc(w->length * sizeof(complex));
	
	w->ftbuf = (complex*)malloc(w->length / 2 * sizeof(complex));
	w->noise_mod = (real_t*)malloc(w->length * sizeof(real_t));
}
/// <summary>
/// 释放维纳滤波器内存
/// </summary>
/// <param name="w">维纳滤波器</param>
void wiener_release(wiener* w)
{
	free(w->signal);
	free(w->signal_frequency);
	free(w->noise);
	free(w->noise_frequency);
	free(w->observe);
	free(w->observe_frequency);
	free(w->ftbuf);
	free(w->noise_mod);
	w->ftbuf = NULL;
	w->signal = NULL;
	w->noise = NULL;
	w->signal_frequency = NULL;
	w->noise_frequency = NULL;
	w->observe = NULL;
	w->observe_frequency = NULL;
	w->noise_mod = NULL;
}

/// <summary>
/// 用噪声样本初始化维纳滤波器
/// </summary>
/// <param name="w">维纳滤波器</param>
/// <param name="noise">噪声数组</param>
/// <param name="len">数组长度</param>
void wiener_init(wiener w, real_t* noise, int len)
{
	if (len > w.length)len = w.length;
	for (int i = 0; i < len; i++) {
		w.noise[i].real = noise[i];
		w.noise[i].imag = 0;
	}
	for (int i = len; i < w.length; i++) {
		w.noise[i].real = 0;
		w.noise[i].imag = 0;
	}

	fft(w.noise, w.noise_frequency, w.ftbuf, w.log2_length);
	for (int i = 0; i < w.length; i++) {
		w.noise_mod[i] = complex_modSquare(w.noise_frequency[i]);

		w.observe[i].real = 0;
		w.observe[i].imag = 0;
	}

	

}


/// <summary>
/// 对维纳滤波器进行滤波。
/// 先在外部更新observe数组。
/// 然后滤波器先计算observe的频谱，减去噪声频谱后得到signal频谱，再根据signal频谱计算signal。
/// 滤波后的数据会放到signal数组。
/// </summary>
/// <param name="w">维纳滤波器</param>
void wiener_filter(wiener w)
{
	fft(w.observe, w.observe_frequency, w.ftbuf, w.log2_length);
	real_t observe_mod;

	for (int i = 0; i < w.length; i++) {
		observe_mod = complex_modSquare(w.observe_frequency[i]);
		observe_mod = (observe_mod - w.noise_mod[i]) / observe_mod;
		if (observe_mod < 0)observe_mod = 0;
		w.signal_frequency[i] = complex_mul_real(w.observe_frequency[i], observe_mod);
	}
	ifft(w.signal, w.signal_frequency, w.ftbuf, w.log2_length);
}

//测试维纳滤波。 只要将原始数据，噪声频谱，滤波数据都打印出来，excel画图即可。
void test_weina() {
	const int len = 32;
	const int ln2len = 5;

	wiener w;
	wiener_allocate(&w, ln2len);

	real_t noise[32];
	for (int i = 0; i < len; i++) {
		noise[i] = random11() * 10;
	}

	wiener_init(w, noise, len);

	for (int i = 0; i < len; i++) {
		printfs("  \r\n ");
		complex_print(w.noise[i]);
	}

	for (int i = 0; i < len; i++) {
		w.observe[i].real = 20 * sinf(i * 0.1) + random11() * 10;
		w.observe[i].imag = 0;
	}
	wiener_filter(w);

	for (int i = 0; i < len; i++) {
		printfs("  \r\n ");
		complex_print(w.observe[i]);
		printfs("    ");
		complex_print(w.observe_frequency[i]);
		printfs("    ");

		complex_print(w.noise[i]);
		printfs("    ");
		complex_print(w.noise_frequency[i]);
		printfs("    ");

		complex_print(w.signal[i]);
		printfs("    ");
		complex_print(w.signal_frequency[i]);
		printfs("    ");

	}

	wiener_release(&w);

	/*

	*/
}

#include"sl_filter.h"  
//这里用 两个主频率的噪声，加上sigmoid函数。进行测试。同时对比卡尔曼滤波。噪声要归一化。
void test_weina2() {
	const int len = 32;
	const int ln2len = 5;

	wiener w;
	wiener_allocate(&w, ln2len);

	real_t noise[32];
	for (int i = 0; i < len; i++) {
		noise[i] = random11() * 0.3 + sinf(i * 6 / 32.0 + 2) + 2 * sinf(i * 12 / 32.0 + 2) + 3 * sinf(i * 3 / 32.0 + 2);
	}

	kalman_float kf0;
	kalman_float_init(&kf0, 1, 10, 1, 0);

	wiener_init(w, noise, len);

	//for (int i = 0; i < len; i++) {
	//	printfs("  \r\n ");
	//	complex_print(w.noise[i]);
	//}

	for (int i = 0; i < 500; i++) {
		real_t s = 10.0 / (1 + expf(-(i - 200) * 0.1));
		real_t n = random11() * 0.3 + sinf(i * 6 / 32.0 + 2) + 2 * sinf(i * 12 / 32.0 + 2);

		w.observe[i % w.length].real = s + n;
		w.observe[i % w.length].imag = 0;
		wiener_filter(w);



		printfs("  \r\n ,%d, s=,%f ,n=, %f, ", i, s, n);
		complex_print(w.observe[i % w.length]);
		complex_print(w.signal[i % w.length]);

		kalman_float_filter(&kf0, w.observe[i % w.length].real);
		printfs(" kalman, %f ,", kf0.now_state);
	}

	wiener_release(&w);

}






//











